/*
 * @Author       : LJenius
 * @Date         : 2022-04-20 18:53:25
 * @LastEditors  : Zybz
 * @LastEditTime : 2022-04-24 14:39:37
 * @FilePath     : \dji_driver_v2.0\Core\Src\command.c
 * @Description  : 电机控制模式配置
 *
 * Copyright (c) 2022 by BUPT RobotTeam, All Rights Reserved.
 */

#include "command.h"
#include "oslib_uart.h"
#include "ctrl.h"
#include <main.h>
/**
 * @brief  电机开启
 * @param n {0 1 2 3}
 * @return {*}
 */
void MotorOn(int n)
{
    if (drivers[n].controlMode == POSITION_CONTROL_MODE)
        drivers[n].posCtrl.desirePos = drivers[n].posCtrl.actulPos;

    if (drivers[n].controlMode == SPEED_CONTROL_MODE)
        drivers[n].velCtrl.desireSpeed = 0.0f;
    reset_PID(&drivers[n].velCtrl.speed_pid);
    reset_PID(&drivers[n].posCtrl.pos_pid);

    drivers[n].status = ENABLE;

    // TODO gpio
}
/**
 * @brief  电机关闭
 * @param n {0 1 2 3}
 * @return {*}
 */
void MotorOff(int n)
{
    drivers[n].status = DISABLE;
    // TODO gpio
}

/**
 * @brief  速度环配置(带电机初始化)
 * @param num 输入{1 2 3 4}， 对应{0 1 2 3}
 * @return {*}
 */
void VelLoopCfg(int num)
{
    if (motorType[num] == RM_3508)
    {
        RM_3508_init(&drivers[num], SPEED_CONTROL_MODE);
    }
    else if (motorType[num] == M_2006)
    {
        M_2006_init(&drivers[num], SPEED_CONTROL_MODE);
    }
    else if (motorType[num] == GM_6020)
    {
        GM_6020_init(&drivers[num], SPEED_CONTROL_MODE);
    }
    else
        return;
    MotorOn(num);
}
/**
 * @brief  位置环配置(带电机初始化)
 * @param num 输入{1 2 3 4}， 对应{0 1 2 3}
 * @param vel 位置环速度
 * @return {*}
 */
void PosLoopCfg(int num, int vel)
{
    if (motorType[num] == RM_3508)
    {
        RM_3508_init(&drivers[num], POSITION_CONTROL_MODE);
    }
    else if (motorType[num] == M_2006)
    {
        M_2006_init(&drivers[num], POSITION_CONTROL_MODE);
    }
    else if (motorType[num] == GM_6020)
    {
        GM_6020_init(&drivers[num], POSITION_CONTROL_MODE);
    }
    else
        return;
    drivers[num].posCtrl.PosVel = vel;
    MotorOn(num);
}
/**
 * @brief  配置电机位置环，但不丢失当前编码器数据
 * @param num {0 1 2 3}
 * @param vel
 * @return {*}
 */
void NoInitPosCfg(int num, int vel)
{
    if (motorType[num] == RM_3508)
    {
        drivers[num].controlMode = POSITION_CONTROL_MODE;
        drivers[num].velCtrl.actualSpeed = 0;
        drivers[num].velCtrl.desireSpeed = 0;
        drivers[num].posCtrl.desirePos = drivers[num].posCtrl.actulPos;
        reset_PID(&(drivers[num].posCtrl.pos_pid));
        reset_PID(&(drivers[num].velCtrl.speed_pid));
    }
    else
        return;
    drivers[num].posCtrl.PosVel = vel;
    MotorOn(num);
}
/**
 * @brief  电流环配置
 * @param num {0 1 2 3}
 * @return {*}
 */
void CurLoopCfg(int num)
{
    if (motorType[num] == RM_3508)
    {
        RM_3508_init(&drivers[num], CUR_CONTROL_MODE);
    }
    else if (motorType[num] == M_2006)
    {
        M_2006_init(&drivers[num], CUR_CONTROL_MODE);
    }
    else if (motorType[num] == GM_6020)
    {
        GM_6020_init(&drivers[num], CUR_CONTROL_MODE);
    }
    else
        return;
    MotorOn(num);
}

void LimitVelCfg(int num)
{
    if (motorType[num] == RM_3508)
    {
        drivers[num].controlMode = SPEED_LIMIT_MODE;
        drivers[num].velCtrl.desireSpeed = 0;
        reset_PID(&(drivers[num].posCtrl.pos_pid));
        reset_PID(&(drivers[num].velCtrl.speed_pid));
    }
    // else if (motorType[num] == M_2006)
    // {
    //     M_2006_init(&drivers[num], SPEED_LIMIT_MODE);
    // }
    // else if (motorType[num] == GM_6020)
    // {
    //     GM_6020_init(&drivers[num], SPEED_LIMIT_MODE);
    // }
    else
        return;
    MotorOn(num);
}
/**
 * @brief  找零模式配置
 * @param num {0 1 2 3}
 * @param vel 找零速度(编码器线/ms) 建议在100以下,正负代表方向
 * @param cur 找零电流(mA) 建议在0~3000之间
 * @return {*}
 */
void HomingModeCfg(int num, int vel, float cur)
{
    uprintf("motorId is %d, vel is %d, cur %f\r\n", num, vel, cur);
    if (motorType[num] == RM_3508)
    {
        reset_PID(&(drivers[num].posCtrl.pos_pid));
        reset_PID(&(drivers[num].velCtrl.speed_pid));
        drivers[num].controlMode = HOMING_MODE;
    }
    else if (motorType[num] == M_2006)
    {
        reset_PID(&(drivers[num].posCtrl.pos_pid));
        reset_PID(&(drivers[num].velCtrl.speed_pid));
        drivers[num].controlMode = HOMING_MODE;
    }
    else if (motorType[num] == GM_6020)
    {
        reset_PID(&(drivers[num].posCtrl.pos_pid));
        reset_PID(&(drivers[num].velCtrl.speed_pid));
        drivers[num].controlMode = HOMING_MODE;
    }
    else
        return;
    drivers[num].homingMode.current = cur;
    drivers[num].homingMode.vel = vel;
    MotorOn(num);
}
/**
 * @brief 速度控制， 对速度限幅
 * @param num {0 1 2 3}
 * @param vel 电机速度
 * @return {*}
 */
void SetVelCtrl(int num, int vel)
{
    uprintf("velctrl num = %d, vel = %d\r\n", num, vel);
    drivers[num].velCtrl.desireSpeed = vel;
    drivers[num].velCtrl.desireSpeed = MaxMinLimit(drivers[num].velCtrl.desireSpeed,
                                                   drivers[num].velCtrl.maxSpeed);
}
/**
 * @brief: 位置环控制， 修改位置环参数, 默认带反馈通知到达
 * @param num {0 1 2 3}
 * @param Pos 目标位置
 * @return {*}
 */
void SetPosCtrl(int num, int Pos)
{
    uprintf("posctrl num = %d, pos = %d\r\n", num, Pos);
    drivers[num].posCtrl.desirePos = Pos;
    drivers[num].posCtrl.arrivalInform = 1;
}
/**
 * @brief 电流环控制， 修改电流环参数
 * @param num {0 1 2 3}
 * @param cur 电流值
 * @return {*}
 */
void SetCurCtrl(int num, int cur)
{
    uprintf("curctrl num = %d, cur = %d\r\n", num, cur);
    drivers[num].curCtrl.desireCur = cur / 1000.0f;
    drivers[num].curCtrl.desireCur = MaxMinLimit(drivers[num].curCtrl.desireCur,
                                                 drivers[num].curCtrl.maxCur);
}

float GetVel(int n)
{
    return drivers[n].velCtrl.actualSpeed;
}

float GetCur(int n)
{
    return motors[n].cur;
}

float GetPos(int n)
{
    return drivers[n].posCtrl.actulPos;
}
